import ia.util.LectorImagenes;

import java.util.*;

public class Monticulo {
	private PriorityQueue<Nodo> monticulo;
	private long width, height;

	public Monticulo() {
		this.monticulo = new PriorityQueue<Nodo>();
		this.width = 0;
		this.height = 0;
	}

	public void add(Nodo n) {
		try {
			monticulo.add(n);
		} catch (java.lang.OutOfMemoryError e) {
			System.out.println("Monticulo: " + monticulo.toString()
					+ "\nTamano: " + size());
			System.out.println(e.toString());
		}
	}

	public double size() {
		return monticulo.size();
	}

	@Override
	public String toString() {
		return monticulo.toString();
	}
	
	private Camino construirSolucion(Nodo n){
		Camino res=new Camino();
		Nodo aux=n;
		while(aux.getPadre()!=null){
			res.getCamino().add(0, aux);
			aux=aux.getPadre();
		}
		res.getCamino().add(0, aux);
		return res;
	}
	
	private Nodo seleccionarCandidatoA(int profundidad){
		if (monticulo.peek() != null)	/* Comprobamos si la cabeza es nula */
			if (monticulo.peek().getProfundidad()==profundidad)	/* Si la cabeza tiene la profundidad actual */
				return monticulo.poll();	/* La devolvemos */
			else return new Nodo(new Posicion(-1,-1),0,0,0,0,null);	/* Si no, devolvemos un nodo error */
		else
			return null;	/* Si no, devolvemos null */
	}

	private Nodo seleccionarCandidatoP() {
		return monticulo.poll();
	}
	
	private double calcularCoste(int ng, int pieza) {
		double res = 0, multCost = 0;
		switch (pieza) {
		case 1:
			multCost = 5;
			break;
		case 2:
			multCost = 3;
			break;
		case 3:
			multCost = 6;
			break;
		case 4:
			multCost = 6;
			break;
		default:
			multCost = 0;
		}
		if (ng == 0)
			multCost *= 100;
		if (pieza == 3 || pieza == 4)
			res = ng * 3 * multCost;
		else
			res = ng * 1 * multCost;

		return res;
	}
	
	
	/*
	 * Metodo general de Busqueda mapa: nombre del fichero de la imagen. PosIni:
	 * (x,y) del comienzo del camino. PosFin(x,y) del destino. Estrategia:
	 * Anchura, Costo Uniforme, Profundidad, ProdundidadAcotada,
	 * ProfundidadIterava. Solo si hay profundidad acotada entonces: cota es el
	 * valor maximo de la cota. Solo si hay profundidad iterativa entonces: cota
	 * para la primera iteracion aumentocota para el incremento de cota en
	 * distintas iteraciones numItera el numero de iteraciones maximo.
	 */

	public Camino busqueda(String rutaMapa, Posicion posIni, Posicion posFin,
			String estrategia, long cota, long aumentocota, int numItera, String heuristica) {
		Camino res = new Camino();
		/*
		 * creamos un lector de imagenes y tomamos la matriz de valores de
		 * grises
		 */
		LectorImagenes li = new LectorImagenes();
		int mapa[][] = li.getMapaGS(rutaMapa);
		width = li.getImagen().getWidth(null);
		height = li.getImagen().getHeight(null);

		/*
		 * Tomamos el primer valor de tiempo para comprobar cuanto tarda en
		 * conseguir la solucion
		 */
		double itime, ftime;
		itime = System.currentTimeMillis();
		/* segun la estrategia escogida hacemos una busqueda especializada */
		if (estrategia.equals("profundidad"))
			res = busquedaProfundidad(mapa, posIni, posFin);
		else if (estrategia.equals("profundidadAcotada"))
			res = busquedaProfundidadAcotada(mapa, posIni, posFin, cota);
		else if (estrategia.equals("profundidadIterativa"))
			res = busquedaProfundidadIterativa(mapa, posIni, posFin, cota,
					aumentocota, numItera);
		else if (estrategia.equals("anchura"))
			res = busquedaAnchura(mapa, posIni, posFin);

		/* Tomamos el valor final del tiempo y lo guardamos en la solucion */
		ftime = System.currentTimeMillis() - itime;
		res.setT(ftime/1000);

		return res;
	}

	
	private Camino busquedaProfundidad(int[][] mapa, Posicion posIni,
			Posicion posFin) {
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		monticulo.add(n);

		int profundidadActual = 0;
		double maxNodos = 0;
		int direccion = 1; /* 1 = horizontal, -1 = vertical */
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoP();
			if (n != null) { /* Hay candidatos */
				if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
					/* Ponemos una pieza de avanzar simple y hemos encontrado solucion */
					n.setPieza(1);
					n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
					n.getVisitados().add(posFin);
					res.getCamino().add(n);
					/*
					 * Calculamos el coste de la solucion y el numero maximo de
					 * nodos almacenados
					 */
					res.setC(n.getCoste());
					res.setM(maxNodos);
					solucionEncontrada = true;
				} else { /* Si no es el nodo objetivo */
					
					if (n.getPiezasProbadas() <= 4) { /* Comprobamos si quedan piezas por probar En orden: recta
					 								   * simple, recta doble, curva a izqda curva a dcha */	
						if (n.getPadre() != null) { /* Pixel intermedio */
							n.setPieza(n.getPiezasProbadas());
							n.setCoste(n.getPadre().getCoste()+ calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
						} else { /* Pixel inicial */
							if (n.getPiezasProbadas() == 1 || n.getPiezasProbadas() == 2) {
								n.setPieza(n.getPiezasProbadas());
								n.setCoste(calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
							}
						}
						
						n.setPiezasProbadas(n.getPiezasProbadas() + 1); /* piezasProbadas++ */
						n.getVisitados().add(n.getPos());
						res.getCamino().add(n);
					
						generarCandidatos(n, direccion, profundidadActual, profundidadActual+1);
						profundidadActual++;
						
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
						
						if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1; /* Si hay curva se cambia de direccion */
						
					} else { /* Si hemos probado todas las piezas, volvemos atras */
						if (profundidadActual>=2){
							/* Quitamos la ultima pieza colocada y no valida */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							res.getCamino().remove(profundidadActual);
							/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							res.getCamino().remove(profundidadActual);
							n.setCoste(0);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							n.setPieza(0);
							monticulo.add(n);
						}
					}
				}
			}else{ /* Si no hay candidatos */
				if(profundidadActual>=1){
					/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
					profundidadActual--;
					n=res.getCamino().get(profundidadActual);
					res.getCamino().remove(profundidadActual);
					n.setCoste(0);
					if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
					n.setPieza(0);
					monticulo.add(n);
				}
			}
		}

		return res;
	}
	
	private Camino busquedaProfundidadAcotada(int[][] mapa, Posicion posIni,
			Posicion posFin, long cota) {
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		monticulo.add(n);

		int profundidadActual = 0;
		double maxNodos = 0;
		int direccion = 1; /* 1 = horizontal, -1 = vertical */
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoP();
			
			if (n != null) { /* Hay candidatos */
				if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
					/* Ponemos una pieza de avanzar simple y hemos encontrado solucion */
					n.setPieza(1);
					n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
					n.getVisitados().add(posFin);
					res.getCamino().add(n);
					/*
					 * Calculamos el coste de la solucion y el numero maximo de
					 * nodos almacenados
					 */
					res.setC(n.getCoste());
					res.setM(maxNodos);
					solucionEncontrada = true;
				} else { /* Si no es el nodo objetivo */
					
					if (n.getPiezasProbadas() <= 4) { /* Comprobamos si quedan piezas por probar En orden: recta
					 								   * simple, recta doble, curva a izqda curva a dcha */	
						if (n.getPadre() != null) { /* Pixel intermedio */
							n.setPieza(n.getPiezasProbadas());
							n.setCoste(n.getPadre().getCoste()+ calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
						} else { /* Pixel inicial */
							if (n.getPiezasProbadas() == 1 || n.getPiezasProbadas() == 2) {
								n.setPieza(n.getPiezasProbadas());
								n.setCoste(calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
							}
						}
						
						n.setPiezasProbadas(n.getPiezasProbadas() + 1); /* piezasProbadas++ */

						n.getVisitados().add(n.getPos());
						res.getCamino().add(n);
						if (profundidadActual < cota){ /* Si no hemos llegado a la cota */
							generarCandidatos(n, direccion, profundidadActual,profundidadActual+1);
							profundidadActual++;
						}else{
							/* Cota excedida */
							if (profundidadActual>=2){
								/* Quitamos la ultima pieza colocada y no valida */
								n=res.getCamino().get(profundidadActual);
								if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
								res.getCamino().remove(profundidadActual);
								/* Quitamos el padre porque no ha sido objetivo y esta en la frontera */
								profundidadActual--;
								n=res.getCamino().get(profundidadActual);
								if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
								res.getCamino().remove(profundidadActual);
							}
						}
						
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
						
						if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1; /* Si hay curva se cambia de direccion */
						
					} else { /* Si hemos probado todas las piezas, volvemos atras */
						if (profundidadActual>=2){
							/* Quitamos la ultima pieza colocada y no valida */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							res.getCamino().remove(profundidadActual);
							/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							res.getCamino().remove(profundidadActual);
							n.setCoste(0);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							n.setPieza(0);
							monticulo.add(n);
						}
					}
				}
			}else{ /* Si no hay candidatos */
				if(profundidadActual>=1){
					/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
					profundidadActual--;
					n=res.getCamino().get(profundidadActual);
					res.getCamino().remove(profundidadActual);
					n.setCoste(0);
					if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
					n.setPieza(0);
					monticulo.add(n);
				}else{
					solucionEncontrada=true;
					res.setM(maxNodos);
					System.out.println("Solucion NO encontrada");
				}
			}
		}

		return res;
	}

	private Camino busquedaProfundidadIterativa(int[][] mapa, Posicion posIni,
			Posicion posFin, long cota, long aumentocota, int numItera) {
		Camino res = new Camino();
		int iteracion=0;
		
		boolean solucionEncontrada=false;
		while (iteracion<numItera && !solucionEncontrada){
			System.out.println("Iteracion " + iteracion + " - Cota " + cota);
			res=busquedaProfundidadAcotada(mapa,posIni,posFin,cota);
			if(res.getCamino().isEmpty()){
				cota+=aumentocota;
				iteracion++;
			}else solucionEncontrada=true;
		}
		return res;
	}

	private Camino busquedaAnchura(int[][] mapa, Posicion posIni, Posicion posFin) {
		Camino resOptimo = new Camino();
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Inicializamos la solucion optima con el peor valor */
		resOptimo.setC(999999999999999.0);
		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		n.setDireccion(1);	/* Escogemos horizontal como la primera direccion */
		monticulo.add(n);
		
		/* Definimos el nodo error. Este indicara que quedan candidatos, pero que no son
		 * de la misma profundidad que la actual y tendremos que aumentarla. Es decir,
		 * se han comprobado todos los nodos de un nivel.
		 */
		Nodo error=new Nodo(new Posicion(-1,-1),0,0,0,0,null);
		
		Nodo [] copiasCandidato = new Nodo[4];
		int profundidadActual = 0;
		double maxNodos = 0;
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoA(profundidadActual);
			if (n != null) { /* Hay candidatos */
				if (n.equals(error)){ /* Hemos comprobado todos los candidatos de un nivel */
					profundidadActual++; /* Pasamos al nivel siguiente */
				}else{
					if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
						/* Ponemos una pieza de avanzar simple y hemos encontrado una solucion */
						n.setPieza(1);
						n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
						
						n.getVisitados().add(posFin);
						res=construirSolucion(n);
						/* Calculamos el coste de la solucion y el numero maximo de nodos almacenados */
						res.setC(n.getCoste());
						res.setM(maxNodos);
						/* Comprobamos si es la mejor solucion */
						if (res.getC() < resOptimo.getC())
							resOptimo=res;
						
					} else { /* Si no es el nodo objetivo */
						
						for (int i=1;i<5;i++){ /* Probamos todas las piezas */
							/* Hacemos una copia del candidato para que no se sobrescriban las piezas */
							copiasCandidato[i-1]= new Nodo(n.getPos(),0,0,n.getF(),n.getProfundidad(),n.getPadre());
							copiasCandidato[i-1].setDireccion(n.getDireccion());
							if(copiasCandidato[i-1].getPadre() != null){  /* Pixel intermedio */
								copiasCandidato[i-1].setPieza(i);
								copiasCandidato[i-1].setCoste(copiasCandidato[i-1].getPadre().getCoste()+ calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
							} else { /* Pixel inicial */
								if(i==1 || i==2){
									copiasCandidato[i-1].setPieza(i);
									copiasCandidato[i-1].setCoste(calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
								}
							}
							copiasCandidato[i-1].getVisitados().add(copiasCandidato[i-1].getPos());
							/* Si el coste aun es menor que el de la mejor solucion */
							if (copiasCandidato[i-1].getCoste()<resOptimo.getC())
								generarCandidatos(copiasCandidato[i-1], copiasCandidato[i-1].getDireccion(), profundidadActual,profundidadActual+1);
							else /* Si no */
								/* Se abandona el camino */;
						}
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
					}
				}
			}else{ /* Si no hay candidatos */
				solucionEncontrada=true;
				if (!resOptimo.getCamino().isEmpty())
					System.out.println("Se ha encontrado Solucion");
				else
					System.out.println("No se ha encontrado Solucion");
			}
		}

		return resOptimo;
	}
	
	private Camino busquedaCostoUniforme(int[][] mapa, Posicion posIni, Posicion posFin) {
		Camino resOptimo = new Camino();
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Inicializamos la solucion optima con el peor valor */
		resOptimo.setC(500.0);
		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		n.setDireccion(1);	/* Escogemos horizontal como la primera direccion */
		monticulo.add(n);
		
		/* Definimos el nodo error. Este indicara que quedan candidatos, pero que no son
		 * de la misma profundidad que la actual y tendremos que aumentarla. Es decir,
		 * se han comprobado todos los nodos de un nivel.
		 */
		Nodo error=new Nodo(new Posicion(-1,-1),0,0,0,0,null);
		
		Nodo [] copiasCandidato = new Nodo[4];
		int profundidadActual = 0;
		double maxNodos = 0;
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoP();
			if (n != null) { /* Hay candidatos */
				if (n.equals(error)){ /* Hemos comprobado todos los candidatos de un nivel */
					profundidadActual++; /* Pasamos al nivel siguiente */
				}else{
					if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
						/* Ponemos una pieza de avanzar simple y hemos encontrado una solucion */
						n.setPieza(1);
						n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
						res=construirSolucion(n);
						n.getVisitados().add(posFin);
						/* Calculamos el coste de la solucion y el numero maximo de nodos almacenados */
						res.setC(n.getCoste());
						res.setM(maxNodos);
						/* Comprobamos si es la mejor solucion */
						if (res.getC() < resOptimo.getC())
							resOptimo=res;
					} else { /* Si no es el nodo objetivo */
						
						for (int i=1;i<5;i++){ /* Probamos todas las piezas */
							/* Hacemos una copia del candidato para que no se sobrescriban las piezas */
							copiasCandidato[i-1]= new Nodo(n.getPos(),0,0,n.getF(),n.getProfundidad(),n.getPadre());
							copiasCandidato[i-1].setDireccion(n.getDireccion());
							if(copiasCandidato[i-1].getPadre() != null){  /* Pixel intermedio */
								copiasCandidato[i-1].setPieza(i);
								copiasCandidato[i-1].setCoste(copiasCandidato[i-1].getPadre().getCoste()+ calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
							} else { /* Pixel inicial */
								if(i==1 || i==2){
									copiasCandidato[i-1].setPieza(i);
									copiasCandidato[i-1].setCoste(calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
								}
							}
							copiasCandidato[i-1].getVisitados().add(copiasCandidato[i-1].getPos());
							/* Si el coste aun es menor que el de la mejor solucion */
							if (copiasCandidato[i-1].getCoste()<resOptimo.getC())
								generarCandidatos(copiasCandidato[i-1], copiasCandidato[i-1].getDireccion(),
										profundidadActual, copiasCandidato[i-1].getCoste());
										/* Como es busqueda con costo uniforme, le pasamos el coste de colocar
										 * la pieza en el valor que tendra F en los hijos */
							else /* Si no */
								/* Se abandona este camino*/ ;
						}
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
					}
				}
			}else{ /* Si no hay candidatos */
				solucionEncontrada=true;
				if (!resOptimo.getCamino().isEmpty())
					System.out.println("Se ha encontrado Solucion");
				else
					System.out.println("No se ha encontrado Solucion");
			}
		}

		return resOptimo;
	}
	
	private void generarCandidatos(Nodo n, int direccion, int profundidadActual, double f) {
		Nodo candidato;
		Posicion p=new Posicion(-1, -1);
		int sentido=-500; /* En caso de que venga un nodo sin direccion se descarta sacandolo de la imagen */
		/* Si la direccion es horizontal */
		if (direccion==1){
			if (n.getPadre()!=null){
				if(n.getPos().getX()<n.getPadre().getPos().getX())
					sentido=-1;
				else if (n.getPos().getX()>n.getPadre().getPos().getX())
					sentido=1;
			}else sentido=1; /* Nodo inicial, direccion horizontal, sentido dcha */
			switch (n.getPieza()){
				case 1:
					p = new Posicion(n.getPos().getX() + 1 * sentido, n.getPos().getY());
					break;
				case 2:
					p = new Posicion(n.getPos().getX() + 2 * sentido, n.getPos().getY());
					break;
				case 3:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() + 1 * sentido);
					break;
				case 4:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() - 1 * sentido);
					break;
				default:
					/* Sin Pieza */ ;
					break;
			}
		}
		
		/* Si la direccion es vertical */
		if (direccion==-1){
			if(n.getPadre()!=null){
				if (n.getPos().getY()<n.getPadre().getPos().getY())
					sentido=-1;
				else if (n.getPos().getY()>n.getPadre().getPos().getY())
					sentido=1;
			} else sentido = 1; /* Nodo inicial, direccion vertical, sentido abajo */
			
			switch (n.getPieza()){
				case 1:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() + 1 * sentido);
					break;
				case 2:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() + 2 * sentido);
					break;
				case 3:
					p = new Posicion(n.getPos().getX() + 1 * sentido, n.getPos().getY());
					break;
				case 4:
					p = new Posicion(n.getPos().getX() - 1 * sentido, n.getPos().getY());
					break;
			}
		}
	
		
		if (p.getX()<width &&  p.getX()>=0 && p.getY()<height && p.getY()>=0){
			/* Si esta dentro de la imagen */
			candidato=new Nodo(p,0,0,f,profundidadActual+1,n);
			if (n.getPieza() == 3 || n.getPieza() == 4) candidato.setDireccion(n.getDireccion()*-1); /* Si hay curva se cambia de direccion */
			else candidato.setDireccion(n.getDireccion());
			
			/* Si no esta visitado */
			if (!n.getVisitados().contains(p))
				monticulo.add(candidato);
			else /* Posicion ya visitada */;
		}
	}
}
